/**
 * @file RobotCtrlType.cpp
 * @author your name (you@domain.com)
 * @brief
 * @version 0.1
 * @date 2022-03-21
 *
 * @copyright Copyright (c) 2022
 *
 */
#include <common/DataType/RobotCtrlType.h>

void convertToMsg(const LegData* data, common::LegData* _data) {
    for (int leg = 0; leg < 4; leg++) {
        _data->flags[leg] = data->flags[leg];
        _data->q_abad[leg] = data->q_abad[leg];
        _data->q_hip[leg] = data->q_hip[leg];
        _data->q_knee[leg] = data->q_knee[leg];
        _data->qd_abad[leg] = data->qd_abad[leg];
        _data->qd_hip[leg] = data->qd_hip[leg];
        _data->qd_knee[leg] = data->qd_knee[leg];
    }
}
void convertToMsg(const RobotCommand* cmd, common::RobotCommand* _cmd) {
    for (int leg = 0; leg < 4; leg++) {
        _cmd->flags[leg] = cmd->flags[leg];

        _cmd->kd_abad[leg] = cmd->kd_abad[leg];
        _cmd->kd_hip[leg] = cmd->kd_hip[leg];
        _cmd->kd_knee[leg] = cmd->kd_knee[leg];

        _cmd->kp_abad[leg] = cmd->kp_abad[leg];
        _cmd->kp_hip[leg] = cmd->kp_hip[leg];
        _cmd->kp_knee[leg] = cmd->kp_knee[leg];

        _cmd->q_des_abad[leg] = cmd->q_des_abad[leg];
        _cmd->q_des_hip[leg] = cmd->q_des_hip[leg];
        _cmd->q_des_knee[leg] = cmd->q_des_knee[leg];

        _cmd->qd_des_abad[leg] = cmd->qd_des_abad[leg];
        _cmd->qd_des_hip[leg] = cmd->qd_des_hip[leg];
        _cmd->qd_des_knee[leg] = cmd->qd_des_knee[leg];

        _cmd->tau_abad_ff[leg] = cmd->tau_abad_ff[leg];
        _cmd->tau_hip_ff[leg] = cmd->tau_hip_ff[leg];
        _cmd->tau_knee_ff[leg] = cmd->tau_knee_ff[leg];

        _cmd->mode_abad[leg] = cmd->mode_abad[leg];
        _cmd->mode_hip[leg] = cmd->mode_hip[leg];
        _cmd->mode_knee[leg] = cmd->mode_knee[leg];      
    }
}
void convertFromMsg(const common::LegDataConstPtr data, LegData* _data) {
    for (int leg = 0; leg < 4; leg++) {
        _data->flags[leg] = data->flags[leg];
        _data->q_abad[leg] = data->q_abad[leg];
        _data->q_hip[leg] = data->q_hip[leg];
        _data->q_knee[leg] = data->q_knee[leg];
        _data->qd_abad[leg] = data->qd_abad[leg];
        _data->qd_hip[leg] = data->qd_hip[leg];
        _data->qd_knee[leg] = data->qd_knee[leg];
    }
}
void convertFromMsg(const common::RobotCommandConstPtr cmd, RobotCommand* _cmd) {
    for (int leg = 0; leg < 4; leg++) {
        _cmd->flags[leg] = cmd->flags[leg];

        _cmd->kd_abad[leg] = cmd->kd_abad[leg];
        _cmd->kd_hip[leg] = cmd->kd_hip[leg];
        _cmd->kd_knee[leg] = cmd->kd_knee[leg];

        _cmd->kp_abad[leg] = cmd->kp_abad[leg];
        _cmd->kp_hip[leg] = cmd->kp_hip[leg];
        _cmd->kp_knee[leg] = cmd->kp_knee[leg];

        _cmd->q_des_abad[leg] = cmd->q_des_abad[leg];
        _cmd->q_des_hip[leg] = cmd->q_des_hip[leg];
        _cmd->q_des_knee[leg] = cmd->q_des_knee[leg];

        _cmd->qd_des_abad[leg] = cmd->qd_des_abad[leg];
        _cmd->qd_des_hip[leg] = cmd->qd_des_hip[leg];
        _cmd->qd_des_knee[leg] = cmd->qd_des_knee[leg];

        _cmd->tau_abad_ff[leg] = cmd->tau_abad_ff[leg];
        _cmd->tau_hip_ff[leg] = cmd->tau_hip_ff[leg];
        _cmd->tau_knee_ff[leg] = cmd->tau_knee_ff[leg];

        _cmd->mode_abad[leg] = cmd->mode_abad[leg];
        _cmd->mode_hip[leg] = cmd->mode_hip[leg];
        _cmd->mode_knee[leg] = cmd->mode_knee[leg];      
    }
}
